#include "servocontroller.h"
#include <QDebug>

ServoController::ServoController(QObject *parent)
    : QObject(parent),
      serialPort(new QSerialPort(this)),
      portName("/dev/ttyS5"),
      baudRate(115200)
{
    // 连接串口数据接收信号
    connect(serialPort, &QSerialPort::readyRead, this, &ServoController::readData);
}

ServoController::ServoController(const QString &port, int baudrate, QObject *parent)
    : QObject(parent),
      serialPort(new QSerialPort(this)),
      portName(port),
      baudRate(baudrate)
{
    // 连接串口数据接收信号
    connect(serialPort, &QSerialPort::readyRead, this, &ServoController::readData);
}

ServoController::~ServoController()
{
    close();
    delete serialPort;
}

bool ServoController::open() {
    if (serialPort->isOpen())
        return true;

    serialPort->setPortName(portName);
    serialPort->setBaudRate(baudRate);
    serialPort->setDataBits(QSerialPort::Data8);
    serialPort->setParity(QSerialPort::NoParity);
    serialPort->setStopBits(QSerialPort::OneStop);
    serialPort->setFlowControl(QSerialPort::NoFlowControl);

    if (!serialPort->open(QIODevice::ReadWrite)) {
        qWarning() << "打开串口失败:" << serialPort->errorString();
        return false;
    }
    
    // 清空缓冲区
    receiveBuffer.clear();
    return true;
}

void ServoController::close() {
    if (serialPort->isOpen()) {
        serialPort->close();
    }
}

bool ServoController::sendData(const QString &data) {
    if (!serialPort->isOpen()) {
        qWarning() << "串口未打开，无法发送数据";
        return false;
    }

    QByteArray byteData = data.toUtf8();
    if (serialPort->write(byteData) != byteData.size()) {
        qWarning() << "串口发送失败:" << serialPort->errorString();
        return false;
    }

    return serialPort->waitForBytesWritten(100);
}

void ServoController::readData() {
    // 读取所有可用数据
    QByteArray newData = serialPort->readAll();
    receiveBuffer.append(newData);
    
    // 调试输出
    qDebug() << "Received raw data:" << newData.toHex();
    
    // 处理缓冲区数据
    while (!receiveBuffer.isEmpty()) {
        // 查找起始字节 0xAA
        int startIndex = receiveBuffer.indexOf(char(0xAA));
        if (startIndex == -1) {
            // 没有找到有效起始字节，清空缓冲区
            receiveBuffer.clear();
            break;
        }
        
        // 移除起始字节前的无效数据
        if (startIndex > 0) {
            receiveBuffer = receiveBuffer.mid(startIndex);
        }
        
        // 检查数据包长度是否足够
        if (receiveBuffer.size() < 5) {
            break; // 等待更多数据
        }
        
        // 检查结束字节是否为 0xFF
        if (static_cast<quint8>(receiveBuffer.at(4)) != 0xFF) {
            // 无效结束字节，丢弃第一个字节并继续处理
            receiveBuffer = receiveBuffer.mid(1);
            continue;
        }
        
        // 提取有效数据包
        quint8 state = static_cast<quint8>(receiveBuffer.at(1));
        quint8 col = static_cast<quint8>(receiveBuffer.at(2));
        quint8 row = static_cast<quint8>(receiveBuffer.at(3));
        
        // 发射接收到的命令信号
        emit receivedCommand(state, row, col);
        
        // 移除已处理的数据包
        receiveBuffer = receiveBuffer.mid(5);
    }
}

void ServoController::W_on() { sendData("a"); }
void ServoController::B_on() { sendData("b"); }
void ServoController::DCT_on() { sendData("c"); }
void ServoController::DCT_off() { sendData("d"); }
